/*!
  @file vis_odometer.h
  @copyright 2013 Kubota Lab. All rights resereved.
*/

#ifndef _VISOD_H_
#define _VISOD_H_

#include <cmath>
#include <exception>
#include <vector>

#include <boost/scoped_ptr.hpp>
#include <boost/shared_ptr.hpp>

#include <opencv2/core/core.hpp>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/ros/conversions.h>

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <tf/transform_broadcaster.h>

#include "mrover_vo/camera_params.h"
#include "mrover_vo/feature.h"
#include "mrover_vo/feature_extractor.h"
#include "mrover_vo/motion_estimator.h"

namespace vo {

/*! 
 *  @brief  Visual Odometry solver
 *  @author Kyohei Otsu <kyon@ac.jaxa.jp>
 *  @date   2013
 *  
 *  detailed documentaiton for this class
 */

class VisOdometer {
  typedef typename Feature::Ptr FeaturePtr;
  typedef pcl::PointCloud<pcl::PointXYZ> Cloud;
  typedef typename Cloud::Ptr CloudPtr;

 public:
  typedef boost::scoped_ptr<VisOdometer> Ptr;


  VisOdometer()
    : extractor_(new FeatureExtractor)
    , estimator_(new MotionEstimator)
    , frame_cnt_(0)
    , img_vec_(2)
  { 
  }

  ~VisOdometer() { }


  void compute(cv::Mat &relative_pose);

  void setCameraParams(const CameraParams::ConstPtr &cam_params) { 
    cam_params_ = cam_params; 
    extractor_->setCameraParams(cam_params_);
    estimator_->setCameraParams(cam_params_);
  }

  void setImages(const cv::Mat &left_img, const cv::Mat &right_img) {
    img_vec_[0] = left_img;
    img_vec_[1] = right_img;
    features_.push_back(FeaturePtr(new Feature));
    ++frame_cnt_;
  }

  inline void resetPose() {
    pose_ = cam_params_->wT[0].clone();
    pose_.at<float>(0, 3) = 0;
    pose_.at<float>(1, 3) = 0;
    pose_.at<float>(2, 3) = 0;
    ROS_INFO_STREAM("init pose" << std::endl << pose_);
  }

  //! Enables mask for feature detection
  /*! This function generates a mask to the sky region specified as v=0..ratio*image_height */
  void enableMaskForFeatureDetection(float ratio=0.25) {
    if (!cam_params_) {
      ROS_ERROR("error in enabling mask. please set the camera params first");
      return;
    }
    
    cv::Size sz(cam_params_->image_size.width, ratio * cam_params_->image_size.height);
    cv::Mat mask = cv::Mat::zeros(cam_params_->image_size, CV_8UC1);
    cv::Mat mask_roi = mask(cv::Rect(cv::Point(0, 0), sz));
    mask_roi = cv::Mat::ones(mask_roi.size(), mask_roi.type()) * 255;
    extractor_->setMask(~mask);
  }

  //! returns the PointCloud of the recent features
  void getCloud(Cloud &cloud) { getFeature()->getCloud(cloud); }

  //! returns the sensor_msgs::PointCloud2 of the recent features
  void getCloudMsg(sensor_msgs::PointCloud2 &msg) {
    CloudPtr cloud(new Cloud);
    getCloud(*cloud);
    pcl::toROSMsg(*cloud, msg);
  }

  //! shows the recent images
  void showImages() { showImages(img_vec_, "images"); }

  //! shows two images in a window
  void showImages(const std::vector<cv::Mat> &imgs, const std::string &winname);

  //! shows the recent images with plots of keypoints 
  void showKeyPoints();
  
  //! shows the recent images with plots of keypoints 
  void showMatches();

  void showTracks(bool show_inliers=false);

  void showStatistics() {
    ROS_INFO("id %d, ", frame_cnt_);
  }

  void publishTF();


 private:
  bool initCompute() {
    if (!img_vec_[0].data || !img_vec_[1].data || !cam_params_) return false;
    return true;
  }

  //! Returns feature pointer at time (t-|pos|)
  /*! If called without a parameter, the function returns the pointer to the 
   * current feature. Given any pos given, it returns the pointer to the 
   * previous feature at time (t-|pos|).
   */
  FeaturePtr getFeature(int pos=0) { 
    try {
      return Feature::Ptr(features_.at(frame_cnt_ - 1 - std::abs(pos)));
    }
    catch (std::exception &e) {
      ROS_ERROR("getFeatures: out of bound (%d of %d)", frame_cnt_ - 1 - std::abs(pos), frame_cnt_);
      ROS_BREAK();
    }
    return Feature::Ptr(); //dummy
  }
  
  //! shows the 1/4-scaled image 
  void showSmallImage(const cv::Mat &orig_img, const std::string &winname);


  FeatureExtractor::Ptr extractor_;
  MotionEstimator::Ptr estimator_;
  tf::TransformBroadcaster tf_pub_;

  CameraParams::ConstPtr cam_params_;

  size_t frame_cnt_;

  std::vector<cv::Mat> img_vec_;
  std::vector<FeaturePtr> features_;
  cv::Mat pose_;  //!< = wTl
};

} // end of namespace

#endif
